/*
 * @Author: your name
 * @Date: 2021-06-04 22:40:25
 * @LastEditTime: 2021-08-01 21:13:30
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \TEST\Core\mycode\control.c
 */
#include "control.h"
#include "tim.h"
#include "usart.h"
#include "math.h"
#include"openmv.h"

#include "float_transmit.h"
#define a_PARAMETER          (0.095f)
#define b_PARAMETER          (0.086f)
// #define START_PWM            (int)8400
#define MOTOR_PWM_LOW        (int)0
PID_S pid_s = {
        {0,0,HHPD1,HHID1,HHDD1,0,0,0,0,0,0,0,fpSumE_max_d,1000,7800,7800,7800,0},
        {0,0,HHPV1,HHIV1,HHDV1,0,0,0,0,0,0,0,fpSumE_max_v,10000,7800,7800,7800,0},
        {0,0,HHPD2,HHID2,HHDD2,0,0,0,0,0,0,0,fpSumE_max_d,1000,7800,7800,7800,0},
        {0,0,HHPV2,HHIV2,HHDV2,0,0,0,0,0,0,0,fpSumE_max_v,10000,7800,7800,7800,0},
        {0,0,HHPT,HHIT,HHDT,0,0,0,0,0,0,0,fpSumE_max_t,150,7800,7800,7800,0},
        HHTARGET_SPEED,
        HHTARGET_DISTANCE,
        HHTARGET_FANGKUAI_SPEED
};
LAST_speeds last_speeds= {{0,0,0,0,0},0,0};
SIQU siqu_pwm = {8000,7700,9000,9000,8600,8600,9000,9000};
CONTROL_data control_data = {0,0,0,0};//控制的结果
CHANGE_Data_Rx change_data_rx;//从串口1接收控制参数的接收缓冲区
RECEIVED_data received_data;//接收到的数据的存放区
TARGET_data target_data = {0,0,0};//用来存放方块目标当前的速度和位置
int derta_pwm[4] = {0,0,0,0};
int test_switch = 0;//判断控制代码中走哪个分支的量
int freshen = 0;//控制进行刷新的标志位，当为1的时候，进行刷新，该值从调参串口接收

void freshen_data(void);
/**
 * @description: 用来最终给电机发送pwm
 * @param {int} if_1如果为1给电机1发送pwm
 * @param {int} if_2如果为1给电机2发送pwm
 * @return {*}
 */
void send_pwm_to_motors(int if_1,int if_2)
{
       if (if_1 == 1)
    {

        if (control_data.speed_now[0] > 0)
        {
            int test_pwm = siqu_pwm.M1_front + (int)derta_pwm[0];
            if (test_pwm >= 0)
            {
                TIM1->CCR2 = test_pwm;      //朝前加PWM
                TIM1->CCR1 = MOTOR_PWM_LOW; //朝后不加电平，一直输出低。a
            }
            else
            {
                TIM1->CCR2 = MOTOR_PWM_LOW;
                TIM1->CCR1 = -test_pwm; //朝后不加电平，一直输出低。a
            }
        }
        else if (control_data.speed_now[0] < 0)
        {
            int test_pwm = siqu_pwm.M1_back - (int)derta_pwm[0];
            if (test_pwm >= 0)
            {
                TIM1->CCR1 = test_pwm;
                TIM1->CCR2 = MOTOR_PWM_LOW; //朝前不加电平，一直输出低。
            }
            else
            {
                TIM1->CCR1 = MOTOR_PWM_LOW;
                TIM1->CCR2 = -test_pwm; //朝后不加电平，一直输出低。a
            }
        }
        else
        {
            if (derta_pwm[0] >= 0)
            {
                int test_pwm = siqu_pwm.M1_front + (int)derta_pwm[0];
                if (test_pwm >= 0)
                {
                    TIM1->CCR2 = test_pwm;      //朝前加PWM
                    TIM1->CCR1 = MOTOR_PWM_LOW; //朝后不加电平，一直输出低。a
                }
                else
                {
                    TIM1->CCR2 = MOTOR_PWM_LOW;
                    TIM1->CCR1 = -test_pwm; //朝后不加电平，一直输出低。a
                }
            }
            else
            {
                int test_pwm = siqu_pwm.M1_back - (int)derta_pwm[0];
                if (test_pwm >= 0)
                {
                    TIM1->CCR1 = test_pwm;
                    TIM1->CCR2 = MOTOR_PWM_LOW; //朝前不加电平，一直输出低。
                }
                else
                {
                    TIM1->CCR1 = MOTOR_PWM_LOW;
                    TIM1->CCR2 = -test_pwm; //朝后不加电平，一直输出低。a
                }
            }
        }
    }
    if (if_2 == 1)
    {
        if (control_data.speed_now[1] > 0)
        {
            int test_pwm = siqu_pwm.M2_front + (int)derta_pwm[1];
            if (test_pwm >= 0)
            {
                TIM1->CCR3 = test_pwm;      //朝前加PWM
                TIM1->CCR4 = MOTOR_PWM_LOW; //朝后不加电平，一直输出低。a
            }
            else
            {
                TIM1->CCR3 = MOTOR_PWM_LOW;
                TIM1->CCR4 = -test_pwm; //朝后不加电平，一直输出低。a
            }
        }
        else if (control_data.speed_now[1] < 0)
        {
            int test_pwm = siqu_pwm.M2_back - (int)derta_pwm[1];
            if (test_pwm >= 0)
            {
                TIM1->CCR4 = test_pwm;
                TIM1->CCR3 = MOTOR_PWM_LOW; //朝前不加电平，一直输出低。
            }
            else
            {
                TIM1->CCR4 = MOTOR_PWM_LOW;
                TIM1->CCR3 = -test_pwm; //朝后不加电平，一直输出低。a
            }
        }
        else
        {
            if (derta_pwm[0] >= 0)
            {
                int test_pwm = siqu_pwm.M2_front + (int)derta_pwm[1];
                if (test_pwm >= 0)
                {
                    TIM1->CCR3 = test_pwm;      //朝前加PWM
                    TIM1->CCR4 = MOTOR_PWM_LOW; //朝后不加电平，一直输出低。a
                }
                else
                {
                    TIM1->CCR3 = MOTOR_PWM_LOW;
                    TIM1->CCR4 = -test_pwm; //朝后不加电平，一直输出低。a
                }
            }
            else
            {
                int test_pwm = siqu_pwm.M2_back - (int)derta_pwm[1];
                if (test_pwm >= 0)
                {
                    TIM1->CCR4 = test_pwm;
                    TIM1->CCR3 = MOTOR_PWM_LOW; //朝前不加电平，一直输出低。
                }
                else
                {
                    TIM1->CCR4 = MOTOR_PWM_LOW;
                    TIM1->CCR3 = -test_pwm; //朝后不加电平，一直输出低。a
                }
            }
        }
    }
}
/**
 * @description: 对速度值进行滤波,同时得到当前的速度值
 * @param {*}
 * @return {*}
 */
void get_and_smoothing_speed(float speed_now)
{
    control_data.speed_now[0] = speed_now;
    if (last_speeds.times<4)
    {
        last_speeds.wheel_1[last_speeds.times] = control_data.speed_now[0];
        last_speeds.times = last_speeds.times+1;
    }
    else
    {
        last_speeds.wheel_1[last_speeds.times] = control_data.speed_now[0];
        last_speeds.times = 0;
    }
    static float speed;
    speed = 0;
    for (int i = 0;i<5;i++)
    {
        speed = speed + last_speeds.wheel_1[i];
    }
    last_speeds.speed = speed/5.0;
}


// /**
//  * @description:该函数在主函数中调用，用来修改PID参数前提是要使用从openmv接收数据的函数，并在其之后调用 
//  * @param {*}
//  * @return {*}
//  */
// void change_PID_from_openmv(void)
// {
//     pid_s.motor1_d.fpKp = (FP32)openmv_data.class[0].target[0].x;
//     pid_s.motor1_d.fpKi = (FP32)openmv_data.class[0].target[0].y;
//     pid_s.motor1_d.fpKd = (FP32)openmv_data.class[0].target[0].d;

//     pid_s.motor1_v.fpKp = (FP32)openmv_data.class[0].target[1].x;
//     pid_s.motor1_v.fpKi = (FP32)openmv_data.class[0].target[1].y;
//     pid_s.motor1_v.fpKd = (FP32)openmv_data.class[0].target[1].d;

//     pid_s.target_speed = openmv_data.class[0].target[2].y;
//     pid_s.target_distence = openmv_data.class[0].target[2].d;

//     pid_s.motor1_d.fpSumE_max = (FP32)openmv_yuzhi.class[0].yuzhi[0];
//     pid_s.motor1_v.fpSumE_max = (FP32)openmv_yuzhi.class[0].yuzhi[1];
//     siqu_pwm.M1_front = (int)openmv_data.class[1].target[0].x;
//     siqu_pwm.M1_front = (int)openmv_data.class[1].target[0].y;


// }

void test_v_PID(int if_1,int if_2)
{
    get_speed(1500);
    if (if_1 == 1)
    {
        control_data.speed_now[0] = encoder_speed[0];
        pid_s.motor1_v.fpFB = (FP32)control_data.speed_now[0];
        pid_s.motor1_v.fpDes = pid_s.target_speed;
        Cal_PID(&pid_s.motor1_v);
        derta_pwm[0] = pid_s.motor1_v.fpU;
    // float fasong[4];
    // fasong[0] = control_data.speed_now[0];
    // fasong[1] = pid_s.target_speed;
    // fasong[2] = pid_s.motor1_v.fpU;
    // fasong[3] = pid_s.motor1_v.fpKp;
    // float_transmit(&huart3,fasong,4);

    }
    if(if_2 == 1)
    {
        control_data.speed_now[1] = -encoder_speed[1];
        pid_s.motor2_v.fpFB = (FP32)control_data.speed_now[1];
        pid_s.motor2_v.fpDes = pid_s.target_speed;
        Cal_PID(&pid_s.motor2_v);
        derta_pwm[1] = pid_s.motor2_v.fpU;
    }
    send_pwm_to_motors(if_1,if_2);
}

float i_test_sin = 0;
void test_d_PID_sin(int if_1,int if_2)
{
    get_speed(1500);   
    if(if_1 == 1)
    {
    control_data.speed_now[0] = encoder_speed[0];//读取当前的速度
    pid_s.motor1_v.fpFB = (FP32)control_data.speed_now[0];//设置当前的速值
    control_data.distence_now[0] = encoder_distence[0];//读取当前的距离
    pid_s.motor1_d.fpFB = (FP32)control_data.distence_now[0];//设置当前的距离
    // pid_s.motor1_d.fpDes = i*0.02*openmv_data.class[0].target[2].y;//设置目标的距离阶跃
    pid_s.motor1_d.fpDes =1560*sin(i_test_sin*0.02*2*3.14159/received_data.class[0].target[2].d);//设置为正弦运动
    //pid_s.motor1_d.fpDes =openmv_data.class[0].target[2].d;//设置目标的距离
    Cal_PID(&pid_s.motor1_d);//位置环PID计算
    //  print_PID(&pid_s.motor1_d,'d');
    pid_s.motor1_v.fpDes = pid_s.motor1_d.fpU;//位置环计算得到的结果设置为速度环的目标速度
    Cal_PID(&pid_s.motor1_v);
    //  print_PID(&pid_s.motor1_v,'v');
    derta_pwm[0] = (int)pid_s.motor1_v.fpU;//速度环输出的结果作为要输出的PWM值
    // float fasong[4];
    // fasong[0] = control_data.speed_now[0];
    // fasong[1] = pid_s.target_distence;
    // fasong[2] = pid_s.motor1_d.fpU;
    // fasong[4] = pid_s.motor1_v.fpU;
    // float_transmit(&huart3,fasong,4);

    }
    if(if_2 == 1)
    {
    control_data.speed_now[1] =-encoder_speed[1];//读取当前的速度
    pid_s.motor2_v.fpFB = (FP32)control_data.speed_now[1];//设置当前的速值
    control_data.distence_now[1] = -encoder_distence[1];//读取当前的距离
    pid_s.motor2_d.fpFB = (FP32)control_data.distence_now[1];//设置当前的距离
    // pid_s.motor1_d.fpDes = i*0.02*openmv_data.class[0].target[2].y;//设置目标的距离阶跃
    pid_s.motor2_d.fpDes =1560*sin(i_test_sin*0.02*2*3.14159/received_data.class[0].target[2].d);//设置为正弦运动
    //pid_s.motor1_d.fpDes =openmv_data.class[0].target[2].d;//设置目标的距离
    Cal_PID(&pid_s.motor2_d);//位置环PID计算
    //  print_PID(&pid_s.motor1_d,'d');
    pid_s.motor2_v.fpDes = pid_s.motor2_d.fpU;//位置环计算得到的结果设置为速度环的目标速度
    Cal_PID(&pid_s.motor2_v);
    //  print_PID(&pid_s.motor1_v,'v');
    derta_pwm[1] = (int)pid_s.motor2_v.fpU;//速度环输出的结果作为要输出的PWM值
    // float fasong[4];
    // fasong[0] = control_data.speed_now[0];
    // fasong[1] = pid_s.target_distence;
    // fasong[2] = pid_s.motor1_d.fpU;
    // fasong[4] = pid_s.motor1_v.fpU;
    // float_transmit(&huart3,fasong,4);
    }
    i_test_sin = i_test_sin+1.0;
    send_pwm_to_motors(if_1,if_2);
}

void test_d_PID(int if_1,int if_2)
{
    get_speed(1500);
    if(if_1 == 1)
    {

    control_data.speed_now[0] =encoder_speed[0] ;//读取当前的速度
    pid_s.motor1_v.fpFB = (FP32)control_data.speed_now[0];//设置当前的速值
    control_data.distence_now[0] = encoder_distence[0];//读取当前的距离
    pid_s.motor1_d.fpFB = (FP32)control_data.distence_now[0];//设置当前的距离
    // pid_s.motor1_d.fpDes = i*0.02*openmv_data.class[0].target[2].y;//设置目标的距离阶跃
    pid_s.motor1_d.fpDes =pid_s.target_distence;//设置为正弦运动
    //pid_s.motor1_d.fpDes =openmv_data.class[0].target[2].d;//设置目标的距离
    Cal_PID(&pid_s.motor1_d);//位置环PID计算
    //  print_PID(&pid_s.motor1_d,'d');
    pid_s.motor1_v.fpDes = pid_s.motor1_d.fpU;//位置环计算得到的结果设置为速度环的目标速度
    Cal_PID(&pid_s.motor1_v);
    //  print_PID(&pid_s.motor1_v,'v');
    derta_pwm[0] = (int)pid_s.motor1_v.fpU;//速度环输出的结果作为要输出的PWM值
    // float fasong[4];
    // fasong[0] = control_data.speed_now[0];
    // fasong[1] = pid_s.target_distence;
    // fasong[2] = pid_s.motor1_d.fpU;
    // fasong[4] = pid_s.motor1_v.fpU;
    // float_transmit(&huart3,fasong,4);

    }
    if(if_2 = 1)
    {
            control_data.speed_now[1] =-encoder_speed[1];//读取当前的速度
    pid_s.motor2_v.fpFB = (FP32)control_data.speed_now[1];//设置当前的速值
    control_data.distence_now[1] = -encoder_distence[1];//读取当前的距离
    pid_s.motor2_d.fpFB = (FP32)control_data.distence_now[1];//设置当前的距离
    // pid_s.motor1_d.fpDes = i*0.02*openmv_data.class[0].target[2].y;//设置目标的距离阶跃
    pid_s.motor2_d.fpDes =pid_s.target_distence;//设置为正弦运动
    //pid_s.motor1_d.fpDes =openmv_data.class[0].target[2].d;//设置目标的距离
    Cal_PID(&pid_s.motor2_d);//位置环PID计算
    //  print_PID(&pid_s.motor1_d,'d');
    pid_s.motor2_v.fpDes = pid_s.motor2_d.fpU;//位置环计算得到的结果设置为速度环的目标速度
    Cal_PID(&pid_s.motor2_v);
    //  print_PID(&pid_s.motor1_v,'v');
    derta_pwm[1] = (int)pid_s.motor2_v.fpU;//速度环输出的结果作为要输出的PWM值
    // float fasong[4];
    // fasong[0] = control_data.speed_now[0];
    // fasong[1] = pid_s.target_distence;
    // fasong[2] = pid_s.motor1_d.fpU;
    // fasong[4] = pid_s.motor1_v.fpU;
    // float_transmit(&huart3,fasong,4);
    }
    send_pwm_to_motors(if_1,if_2);

}
float i_test_pid_line1 = 0;
float i_test_pid_line2 = 0;
void test_d_PID_line(int if_1,int if_2)
{
    get_speed(1500);
    if(if_1 == 1)
    {
        control_data.speed_now[0] = encoder_speed[0];//读取当前的速度
        pid_s.motor1_v.fpFB = (FP32)control_data.speed_now[0];//设置当前的速值
        control_data.distence_now[0] = encoder_distence[0];//读取当前的距离
        pid_s.motor1_d.fpFB = (FP32)control_data.distence_now[0];//设置当前的距离
    // pid_s.motor1_d.fpDes = i*0.02*openmv_data.class[0].target[2].y;//设置目标的距离阶跃
        pid_s.motor1_d.fpDes =i_test_pid_line1;//设置为正弦运动
    //pid_s.motor1_d.fpDes =openmv_data.class[0].target[2].d;//设置目标的距离
        Cal_PID(&pid_s.motor1_d);//位置环PID计算
    //  print_PID(&pid_s.motor1_d,'d');
        pid_s.motor1_v.fpDes = pid_s.motor1_d.fpU;//位置环计算得到的结果设置为速度环的目标速度
        Cal_PID(&pid_s.motor1_v);
    //  print_PID(&pid_s.motor1_v,'v');
        derta_pwm[0] = (int)pid_s.motor1_v.fpU;//速度环输出的结果作为要输出的PWM值
    // float fasong[4];
    // fasong[0] = control_data.speed_now[0];
    // fasong[1] = pid_s.target_distence;
    // fasong[2] = pid_s.motor1_d.fpU;
    // fasong[4] = pid_s.motor1_v.fpU;
    // float_transmit(&huart3,fasong,4);
    }
    if(if_2 = 1)
    {
    control_data.speed_now[1] = -encoder_speed[1];//读取当前的速度
    pid_s.motor2_v.fpFB = (FP32)control_data.speed_now[1];//设置当前的速值
    control_data.distence_now[1] = -encoder_distence[1];//读取当前的距离
    pid_s.motor2_d.fpFB = (FP32)control_data.distence_now[1];//设置当前的距离
    // pid_s.motor1_d.fpDes = i*0.02*openmv_data.class[0].target[2].y;//设置目标的距离阶跃
    pid_s.motor2_d.fpDes =i_test_pid_line2;//设置为正弦运动
    //pid_s.motor1_d.fpDes =openmv_data.class[0].target[2].d;//设置目标的距离
    Cal_PID(&pid_s.motor2_d);//位置环PID计算
    //  print_PID(&pid_s.motor1_d,'d');
    pid_s.motor2_v.fpDes = pid_s.motor2_d.fpU;//位置环计算得到的结果设置为速度环的目标速度
    Cal_PID(&pid_s.motor2_v);
    //  print_PID(&pid_s.motor1_v,'v');
    derta_pwm[1] = (int)pid_s.motor2_v.fpU;//速度环输出的结果作为要输出的PWM值
    // float fasong[4];
    // fasong[0] = control_data.speed_now[0];
    // fasong[1] = pid_s.target_distence;
    // fasong[2] = pid_s.motor1_d.fpU;
    // fasong[4] = pid_s.motor1_v.fpU;
    // float_transmit(&huart3,fasong,4);

    }
    send_pwm_to_motors(if_1,if_2);
    i_test_pid_line1 =  i_test_pid_line1+pid_s.target_speed;
    i_test_pid_line2 = i_test_pid_line2 + pid_s.target_speed_2;

}

// void test_finial(void)
// {
//     float k = received_data.class[0].target[2].x;//速度规划的三个参数
//     float x0 =  received_data.class[0].target[2].y;
//     float x1 = received_data.class[0].target[2].d;
//     target_data.x_now = openmv_data.class[0].target[0].y;//获取当前的位置
//     control_data.target_positon = target_data.x_now;
//     float speed = (target_data.x_now-target_data.last_x)*50;
//     if(abs(speed)>=100)
//     {
//         if(speed >0)
//         {
//             speed = 100;
//         }
//         else{
//             speed = -100;
//         }
//     }
//     get_and_smoothing_speed(speed);
//     target_data.v = last_speeds.speed;

//     target_data.last_x = target_data.x_now;
//     control_data.target_v = target_data.v;
//     pid_s.target_v.fpFB = (FP32)target_data.v;
//     pid_s.target_v.fpDes = -(FP32)(target_data.x_now-x0)*(target_data.x_now-x1)*k;//用二次函数来规划目标速度
//     Cal_PID(&pid_s.target_v);
//     control_data.speed_now[0] = -get_speed(1,100000);//读取当前的速度
//     pid_s.motor1_v.fpFB = (FP32)control_data.speed_now[0];//设置当前的速值
//     control_data.distence_now[0] = -encoder_distence[0];//读取当前的距离
//     pid_s.motor1_d.fpFB = (FP32)control_data.distence_now[0];//设置当前的距离
//     // pid_s.motor1_d.fpDes = i*0.02*openmv_data.class[0].target[2].y;//设置目标的距离阶跃
//     if (pid_s.target_v.fpU>0)
//     {
//         pid_s.motor1_d.fpDes =(FP32)control_data.distence_now[0]-(FP32)pid_s.target_v.fpU-20;
//     }
//     else if(pid_s.target_v.fpU<0)
//     {
//         pid_s.motor1_d.fpDes =(FP32)control_data.distence_now[0]-(FP32)pid_s.target_v.fpU+20;
//     }else
//     {
//     pid_s.motor1_d.fpDes =(FP32)control_data.distence_now[0]-(FP32)pid_s.target_v.fpU;//设置目标距离为当前的距离加上前一环PID计算得到的距离
//     }
//     //pid_s.motor1_d.fpDes =openmv_data.class[0].target[2].d;//设置目标的距离
//     Cal_PID(&pid_s.motor1_d);//位置环PID计算
//     //  print_PID(&pid_s.motor1_d,'d');
//     pid_s.motor1_v.fpDes = pid_s.motor1_d.fpU;//位置环计算得到的结果设置为速度环的目标速度
//     Cal_PID(&pid_s.motor1_v);
//     //  print_PID(&pid_s.motor1_v,'v');
//     derta_pwm[0] = (int)pid_s.motor1_v.fpU;//速度环输出的结果作为要输出的PWM值
//     // float fasong[4];
//     // fasong[0] = control_data.speed_now[0];
//     // fasong[1] = pid_s.target_distence;
//     // fasong[2] = pid_s.motor1_d.fpU;
//     // fasong[4] = pid_s.motor1_v.fpU;
//     // float_transmit(&huart3,fasong,4);
//     send_pwm_to_motors();

// }
/**
 * @description:执行控制的函数，定时器中断中执行的函数。
 * @param {CONTROL_data} *control_data
 * @return {*}
 */
int if_1 = 0;
int if_2 = 0;
void do_control(CONTROL_data *control_data)
{

    if (test_switch == 1)
    {
     test_v_PID(if_1,if_2);
    }
    else if(test_switch == 2)
    {
         test_d_PID(if_1,if_2);
    }
    else if (test_switch == 3)
    {
        TIM1->CCR1 = 0;
        TIM1->CCR2 = 0;
        TIM1->CCR3 = 0;
        TIM1->CCR4 = 0;
        encoder_distence[0] = 0;
        encoder_distence[1] = 0;
    }
    else if(test_switch == 5)
    {
        test_d_PID_sin(if_1,if_2);
    }
    else if(test_switch == 6)
    {
        test_d_PID_line(1,1);
    }
    else if(test_switch == 7)
    {
    	get_speed(1500);
        TIM1->CCR1 = 0;
        TIM1->CCR2 = 9000;
        TIM1->CCR3 = 9000;
        TIM1->CCR4 = 0;
        control_data->speed_now[1] = -encoder_speed[1];//读取当前的速度
        control_data->speed_now[0] = encoder_speed[0];//读取当前的速度
    }
    else if(test_switch == 8)
    {
        servo2_move_to_angle(servo2.angle);
    }
    else
    {
    	get_speed(1500);
        // float a = 1.11;
        // float_transmit(&huart3, &a, 1);
        control_data->speed_now[1] = -encoder_speed[1];//读取当前的速度
        control_data->speed_now[0] = encoder_speed[0];//读取当前的速度
        
    }

}

/**
 * @description: 定时器10的中断回调函数，用来产生控制周期
 * @param {TIM_HandleTypeDef} *htim
 * @return {*}
 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance==TIM10)
	{
         do_control(&control_data);
	}
}



/**
 * @description: 这个函数主要用来接收PID参数
 * 这个函数用在UART接收中断函数HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)中
 该函数自带串口中断的判断    if(huart == &huart1
 自带串口中断的开启HAL_UART_Receive_IT(&huart1, (uint8_t *)&openmv_a_RxBuffer, 1);
 接收的缓冲区不用再定义，定义在openmv.h中
 所以接收中断回调函数需要#include"openmv.h"
 * @param {*}
 * @return {*}
 */
void receive_control_datas(UART_HandleTypeDef *huart)
{
    // HAL_UART_Transmit(&huart3,&change_data_rx.a_buffer[0],1,1000);
     if(huart == &huart2)
    {
      if(change_data_rx.finished_data == 0)
      {
    	 change_data_rx.data_buffer[change_data_rx.counter] = change_data_rx.a_buffer[0];
        if(change_data_rx.counter==0&&change_data_rx.a_buffer[0]!=0x53)          //�????0号数据不是帧头，跳过
        {
              HAL_UART_Receive_IT(&huart2, (uint8_t *)&change_data_rx.a_buffer, 1);
              return;
        }
        change_data_rx.counter = change_data_rx.counter+1;
        if(change_data_rx.counter==17)
        {  
              if((change_data_rx.data_buffer[0] == 'S')&&(change_data_rx.data_buffer[16]=='T'))
              {
                change_data_rx.finished_data = 1;
                //   int class_num = change_data_rx.data_buffer[2];
                //   int target_num = change_data_rx.data_buffer[3];
                //   char* start =  (char*)(&(received_data.class[class_num].target[target_num]));
                //   memcpy(start,&change_data_rx.data_buffer[4],12);
                //   reput_data_from_uart1();
              }
             change_data_rx.counter=0; //重新赋�?�，准备下一帧数据的接收
        }
      }
     }
    HAL_UART_Receive_IT(&huart2, (uint8_t *)&change_data_rx.a_buffer, 1);
}

void control_data_process(void)
{
    if (change_data_rx.finished_data == 1)
    {   
        int class_num = change_data_rx.data_buffer[2];
        int target_num = change_data_rx.data_buffer[3];
        char* start =  (char*)(&(received_data.class[class_num].target[target_num]));
        memcpy(start,&change_data_rx.data_buffer[4],12);
        reput_data_from_uart1();
        change_data_rx.finished_data = 0;
        freshen_data();
    }  
}
/**
 * @description: 将串口接收到的数据重新放到应该放到的结构体中 要特别注意数据类型的转变,这个函数根据需要的参数进行不断修改
 * @param {*}
 * @return {*}
 */
void reput_data_from_uart1(void)
{
    pid_s.motor1_d.fpKp = (FP32)received_data.class[1].target[1].x;
    pid_s.motor1_d.fpKi = (FP32)received_data.class[1].target[1].y;
    pid_s.motor1_d.fpKd = (FP32)received_data.class[1].target[1].d;
    pid_s.motor1_v.fpKp = (FP32)received_data.class[1].target[2].x;
    pid_s.motor1_v.fpKi = (FP32)received_data.class[1].target[2].y;
    pid_s.motor1_v.fpKd = (FP32)received_data.class[1].target[2].d;

    pid_s.motor2_d.fpKp = (FP32)received_data.class[2].target[1].x;
    pid_s.motor2_d.fpKi = (FP32)received_data.class[2].target[1].y;
    pid_s.motor2_d.fpKd = (FP32)received_data.class[2].target[1].d;
    pid_s.motor2_v.fpKp = (FP32)received_data.class[2].target[2].x;
    pid_s.motor2_v.fpKi = (FP32)received_data.class[2].target[2].y;
    pid_s.motor2_v.fpKd = (FP32)received_data.class[2].target[2].d;

    test_switch = (int)received_data.class[0].target[0].x;
    siqu_pwm.M1_front = (int)received_data.class[0].target[0].y;
    siqu_pwm.M1_back = (int)received_data.class[0].target[0].d;
    siqu_pwm.M2_front = (int)received_data.class[0].target[1].x;
    siqu_pwm.M2_back = (int)received_data.class[0].target[1].y;


    pid_s.target_speed = received_data.class[0].target[1].d;
    pid_s.target_distence = received_data.class[0].target[2].x;
    pid_s.target_speed_2 = received_data.class[2].target[0].y;
    
    freshen = (int)received_data.class[0].target[2].y;
    if_1 = (int)received_data.class[1].target[0].x;
    if_2 = (int)received_data.class[1].target[0].y; 
    servo2.angle = received_data.class[1].target[0].d;
}

/**
 * @description:用来切换任务时刷新数据 
 * @param {*}
 * @return {*}
 */
void freshen_data(void)
{
    if (freshen == 1)
    {
        TIM1->CCR1 = 0;
        TIM1->CCR2 = 0;
        TIM1->CCR3 = 0;
        TIM1->CCR4 = 0;
        control_data.distence_now[0] = 0;
        encoder_distence[0] = 0;
        control_data.speed_now[0] = 0;
        control_data.distence_now[1] = 0;
        encoder_distence[1] = 0;
        control_data.speed_now[1] = 0;
        pid_s.motor1_d.fpSumE = 0;
        pid_s.motor1_v.fpSumE = 0;
        pid_s.motor2_d.fpSumE = 0;
        pid_s.motor2_v.fpSumE = 0;
        freshen = 0;
        i_test_pid_line1= 0;
        i_test_pid_line2 = 0;
        i_test_sin = 0;
    }
    
}
